package de.h_da.fbi.cg.pointcloud.leaf;

import de.h_da.fbi.cg.pointcloud.adt.Edge;
import de.h_da.fbi.cg.pointcloud.adt.Graph;
import de.h_da.fbi.cg.pointcloud.adt.Vertex;
import de.h_da.fbi.cg.pointcloud.adt.VertexList;
import de.h_da.fbi.cg.pointcloud.spatial.EuclideanDistance;
import de.h_da.fbi.cg.pointcloud.spatial.KDTree;
import de.h_da.fbi.cg.pointcloud.spatial.NearestNeighborSearch;
import de.h_da.fbi.cg.pointcloud.spatial.NearestNeighborSearch.Entry;
import de.h_da.fbi.cg.pointcloud.util.PointInConeUtil;
import de.h_da.fbi.cg.pointcloud.util.constants.Constants;

/**
 * @author Nicolas Gramlich
 * @since 11:15:43 - 06.01.2011
 */
public class LeafPointConnector {
	// ===========================================================
	// Constants
	// ===========================================================

	// ===========================================================
	// Fields
	// ===========================================================

	// ===========================================================
	// Constructors
	// ===========================================================

	public LeafPointConnector() {
	}

	// ===========================================================
	// Methods
	// ===========================================================

	public Graph compute(final Graph pGraph) {
		// TODO Maybe use kdTree constructed in GraphUtil.createComponentsWithNearestNeighborEdges
		final KDTree<Float, Vertex, Void> kdTree = new KDTree<Float, Vertex, Void>(3);

		final VertexList vertices = pGraph.getVertices();
		final int vertexCount = vertices.size();
		for(int i = 0; i < vertexCount; i++) {
			kdTree.put(vertices.get(i), null);
		}

		kdTree.balance();

		final NearestNeighborSearch<Float, Vertex, Void> nnSearch = new NearestNeighborSearch<Float, Vertex, Void>(new EuclideanDistance<Float, Vertex>());

		for(int i = 0; i < vertices.size(); i++) {
			final Vertex leaf = vertices.get(i);
			if(leaf.isLeaf()) {
				this.connectLeaf(pGraph, leaf, nnSearch, kdTree);
			}
		}

		return pGraph;
	}

	private void connectLeaf(final Graph pGraph, final Vertex pLeaf, final NearestNeighborSearch<Float, Vertex, Void> nnSearch, final KDTree<Float, Vertex, Void> pKDTree) {
		final Entry<Float, Vertex, Void>[] neighbors = nnSearch.get(pKDTree, pLeaf, Constants.LEAF_CONNECTOR_NEIGHBOR_COUNT, true);

		final Vertex bestLeafConnectionVertex = this.findBestLeafConnectionVertexInCone(pLeaf, neighbors);
		if(bestLeafConnectionVertex != null) {
			final Edge leafConnectionEdge = new Edge(pLeaf, bestLeafConnectionVertex);
			pGraph.addEdge(leafConnectionEdge);
		}
	}

	public Vertex findBestLeafConnectionVertexInCone(final Vertex pLeaf, final Entry<Float, Vertex, Void>[] pNeighbors) {
		final Edge leafEdge = pLeaf.getConnectedEdges().get(0);
		final Vertex preLeafVertex = (leafEdge.mVertexA == pLeaf) ? leafEdge.mVertexB : leafEdge.mVertexA;
		final Vertex coneDirection = pLeaf.sub(preLeafVertex);

		Vertex leafConnectionVertex = null;
		final float leafConnectionVertexDistance = Float.MAX_VALUE;

		for(int i = 0; i < pNeighbors.length; i++) {
			final Vertex neighbor = pNeighbors[i].getKey();
			if(PointInConeUtil.isPointInCone(pLeaf, coneDirection, Constants.CONE_ANGLE, neighbor)) {
				if(leafConnectionVertexDistance > neighbor.getDistanceTo(pLeaf)) {
					leafConnectionVertex = neighbor;
				}
			}
		}

		return leafConnectionVertex;
	}

	// ===========================================================
	// Getter & Setter
	// ===========================================================
}
